/*
*	Project: Aethon
*
*	File: PhysicsManager.cpp
*
*	Author: Jonathan Hersack
*
*	Date: 2/15/2009
*
*   Purpose: The PhysicsManager updates and validates all of the model movements
*   and determines collisions
*/

#include "../include/PhysicsManager.h"

//######################################################
void PhysicsManager::update(vector<Model> *modelListPtr, Terrain *terrainPtr)
{//TODO: break model update into new function
	//updates after checking for model collisions
	terrainCollision(modelListPtr,terrainPtr);
	modelCollision(modelListPtr);
}//end update

//######################################################
vector3f PhysicsManager::generateNewPosition(Model *modelA, vector3f tempVel)
{
	vector3f tempPos = modelA->getPosition();
	tempPos.v[0] += tempVel.v[0];
	tempPos.v[1] += tempVel.v[1];
	tempPos.v[2] += tempVel.v[2];
	return tempPos;
	
}//end gen new pos

//####################################################################
vector3f PhysicsManager::normalizeVector(vector3f vect)
{
	float magnitude = sqrt(vect.v[0]*vect.v[0]+vect.v[1]*vect.v[1]+vect.v[2]*vect.v[2]);
	vector3f unitVect;
	unitVect.v[0]=vect.v[0]/magnitude;
	unitVect.v[1]=vect.v[1]/magnitude;
	unitVect.v[2]=vect.v[2]/magnitude;
return unitVect;
}//end normailzevector

//######################################################
vector3f PhysicsManager::reactModelCollision(Model *model,vector3f tempVel, vector3f tempPos)
{//adjusts tempPosition by radius of collisionsphere in opposite direction of velocity            
	tempVel = normalizeVector(tempVel);//get unit vector for direction
						
//subtract radius of collision sphere in opposite direction of velocity
	tempPos.v[0] -= tempVel.v[0]*model->getCollisionSphere().getRadius();
	tempPos.v[1] -= tempVel.v[1]*model->getCollisionSphere().getRadius();
	tempPos.v[2] -= tempVel.v[2]*model->getCollisionSphere().getRadius();
			 
	return tempPos;		 			 
}//end react model collision

//######################################################
void PhysicsManager::updateModelPosition(Model *modelAPtr, vector3f tempPos)
{//adjusts tempPosition by radius of collisionsphere in opposite direction of velocity             	
	modelAPtr->setPosition(tempPos);
	CollisionSphere testSphere = modelAPtr->getCollisionSphere();
	testSphere.setCenterPosition(tempPos);
	modelAPtr->setCollisionSphere(testSphere);		 
}//end react model collision


//######################################################
void PhysicsManager::modelCollision(vector<Model> *modelListPtr)
{//adjusts tempPosition by radius of collisionsphere in opposite direction of velocity         
int size = modelListPtr->size();
	for(int i = 0; i < size; i++)
	{
		for(int j = 0; j < size; j++)
		{	
		Model *modelAPtr = &(modelListPtr->at(i));
		vector3f tempVel = modelAPtr->getVelocity();
		
		   if( !( (tempVel.v[0] == 0) && (tempVel.v[1] == 0) && (tempVel.v[2] == 0) ) )
			{//model is moving so check for collisions on new positions
			//fprintf(stdout, "moving now");
				vector3f tempPos = generateNewPosition(modelAPtr, tempVel);
				CollisionSphere testSphere = modelAPtr->getCollisionSphere();
				if(hasCollisions)
				{//ignore collision if collisions are off
					
					if((i!=j))
					{//ignore collision if testign agaisnt self
						Model *modelB = &(modelListPtr->at(j));
						
						//used to test if next position collides
						testSphere.setCenterPosition(tempPos);
						modelAPtr->setCollisionSphere(testSphere);

						if(modelAPtr->getCollisionSphere().collidesWith(modelB->getCollisionSphere())==1)
						{
							tempPos = reactModelCollision(modelAPtr, tempVel, tempPos);	
							updateModelPosition(modelAPtr,tempPos);
							//reset velocity to zero
							vector3f zeroVect(0,0,0);
							modelAPtr->setVelocity(zeroVect);
							tempVel = zeroVect;
						}//end if collision		
						else
						{
							if(j==size-1)
							{//if all models have been checked, set pos
								updateModelPosition(modelAPtr,tempPos);
								//reset velocity to zero
								vector3f zeroVect(0,0,0);
								modelAPtr->setVelocity(zeroVect);
								tempVel = zeroVect;
							}//end if all checked	
						}//end else					
						
					}//end if !samemodel

				}//end if hascollisions	
				
				else//if no collisions
				{
					updateModelPosition(modelAPtr,tempPos);
					//reset velocity to zero
					vector3f zeroVect(0,0,0);
					modelAPtr->setVelocity(zeroVect);
					tempVel = zeroVect;
				}//end else
				
			}//end if
		}//end inner for	
	}//end outer for	   
}//end react model collision

//######################################################
void PhysicsManager::terrainCollision(vector<Model> *modelListPtr, Terrain *terrainPtr)
{//adjusts model height to make it on top of terrain
    int size = modelListPtr->size();
	for(int i = 0; i < size; i++)  
	{
		Model *modelPtr = &(modelListPtr->at(i));
		if(modelPtr->getHasGravity())
		{
		vector3f position = modelPtr->getPosition();
		float height = terrainPtr->getHeightAtPoint(position.v[0], position.v[2]);
		position.v[1] = height+modelPtr->getCollisionSphere().getRadius();
		modelPtr->setPosition(position);
		}//end if
	}//edn for	
	
}//end react model collision


